#include "ThreadManager.h"
#include "easylogging++.h"



hnurm::ThreadManager::ThreadManager(const std::string &config_file_path)
{
    InitManager(config_file_path);
}

hnurm::ThreadManager::~ThreadManager()
{
    Exit();
}

void hnurm::ThreadManager::InitManager(const std::string &config_file_path)
{
    cv::FileStorage config(config_file_path, cv::FileStorage::READ);
    if (!config.isOpened())
    {
        LOG(ERROR) << "failed to open config file";
        exit(-1);
    }
    LOG(INFO) << "Reading config file";
    // init all config nodes
    // clang-format off
    _serial_config_node      = config["serial"];
    _camera_config_node      = config["camera"];
    _detector_config_node    = config["detector"];
    _processor_config_node   = config["processor"];
    _compensator_config_node = config["compensator"];
    _solver_config_node      = _processor_config_node["solver"];
    // clang-format on

    // init all modules
    // clang-format off
    _camera             = std::make_unique<HKcam>(_camera_config_node);
    _detector           = std::make_unique<Detector>(_detector_config_node);
    _processor          = std::make_unique<Processor>(_processor_config_node);
    _serial_codec       = std::make_unique<SerialCodec>(_serial_config_node);
    _compensator        = std::make_unique<Compensator>(_compensator_config_node);
    _processor->tsolver = std::make_unique<TSolver>(_solver_config_node, _camera->GetCamName());

#ifdef DEBUG_VOFA
    _tcp_logger         = std::make_unique<TCP_Logger>();
#endif

    // clang-format on
    LOG(INFO) << "================================================";
    LOG(INFO) << "Successfully initialised all modules ";
    LOG(INFO) << "================================================";
}


[[noreturn]] void hnurm::ThreadManager::ImageAcquisitionThread()
{
    // 设置录制器的计时器
    time_t start_time = time(0);
    int duration = 5 * 60; // 5分钟，以秒为单位
    while (true)
    {
        ImgInfo tmp_img;
        TIMEIT_ID(
                CAMERA,
                if (_camera->SendFrame(tmp_img)) {
                    LOG(DEBUG) << _image_buffer.Update(tmp_img);
                    // 内录代码
                    if(this->_camera->_if_record && time(0)- start_time <duration){
                        // this->_camera->video_writer << tmp_img.img;
                    }
                } else if (this->_camera->SendFromVideoCapture(tmp_img)) {
                    LOG(DEBUG) << _image_buffer.Update(tmp_img);
                } else {
                    CLOG(DEBUG, "camera") << "read an empty frame from camera";
                    std::this_thread::sleep_for(5ms);
                })

#ifdef DEBUG_SHOW
        if (!tmp_img.img.empty()) {
            cv::namedWindow("send_image", cv::WINDOW_AUTOSIZE);
            cv::imshow("send_image", tmp_img.img);
        }
        cv::waitKey(1);
#endif
    }
}

[[noreturn]] void hnurm::ThreadManager::ImageProcessingThread()
{
#ifdef DEBUG_PARAMETERS
    //调参中心
    cv::namedWindow("parameters center", cv::WINDOW_AUTOSIZE);
    cv::namedWindow("parameters center2",cv::WINDOW_AUTOSIZE);
    int light_min_ratiox100 = int(_detector->l.min_ratio * 100);
    int light_max_ratiox100 = int(_detector->l.max_ratio * 100);
    int light_max_angle = int(_detector->l.max_angle);
    int armor_min_light_ratiox10 = int(_detector->a.min_light_ratio * 10);
    int armor_min_small_center_distancex10 = int(_detector->a.min_small_center_distance * 10);
    int armor_max_small_center_distancex10 = int(_detector->a.max_small_center_distance * 10);
    int armor_min_large_center_distancex10 = int(_detector->a.min_large_center_distance * 10);
    int armor_max_large_center_distancex10 = int(_detector->a.max_large_center_distance * 10);
    int armor_max_angle = int(_detector->a.max_angle);
    int camera_bias_z = int(_processor->tsolver->get_cam_bias()[0]);
    int camera_bias_y = int(_processor->tsolver->get_cam_bias()[1]);
    int channel_delay_x100 = int(this->_compensator->channel_delay_ * 100);
    //设置敌方颜色
    cv::createTrackbar("_enemy_color", "parameters center", nullptr, 2);
    //设置图像识别相关参数，对应初始化参数在config.yaml中，若是小数的乘以10的倍数进行处理
    cv::createTrackbar("min_lightness", "parameters center", &(_detector->min_lightness), 255);
    cv::createTrackbar("light_min_ratio_x100", "parameters center", &light_min_ratiox100, 50);
    cv::createTrackbar("light_max_ratio_x100", "parameters center", &light_max_ratiox100, 100);
    cv::createTrackbar("light_max_angle", "parameters center", &light_max_angle, 90);
    cv::createTrackbar("armor_min_light_ratio_x10", "parameters center", &armor_min_light_ratiox10, 10);
    cv::createTrackbar("armor_min_small_center_distance_x10", "parameters center", &armor_min_small_center_distancex10, 50);
    cv::createTrackbar("armor_max_small_center_distance_x10", "parameters center", &armor_max_small_center_distancex10, 100);
    cv::createTrackbar("armor_min_large_center_distance_x10", "parameters center", &armor_min_large_center_distancex10, 100);
    cv::createTrackbar("armor_max_large_center_distance_x10", "parameters center", &armor_max_large_center_distancex10, 100);
    cv::createTrackbar("armor_max_angle", "parameters center", &armor_max_angle, 90);
    cv::createTrackbar("camera_bias_z", "parameters center", &camera_bias_z, 500);
    cv::createTrackbar("camera_bias_y", "parameters center", &camera_bias_y, 500);


    // 实现正负5度的调节
    cv::createTrackbar("positive_bias_pitch_x100", "parameters center2", nullptr, 500);
    cv::createTrackbar("positive_bias_yaw_x100", "parameters center2", nullptr, 500);
    cv::createTrackbar("negative_bias_pitch_x100", "parameters center2", nullptr, 500);
    cv::createTrackbar("negative_bias_yaw_x100", "parameters center2", nullptr, 500);
    cv::createTrackbar("channel_delay_x100", "parameters center2", &channel_delay_x100, 200);
#endif

    while (true)
    {

        ImgInfo raw_img;
        VisionRecvData recv;
        VisionSendData send;
        TargetInfo tmp_target;

        //未读取到图片
        if (!_image_buffer.Get(raw_img))
        {
            CLOG(DEBUG, "process") << "read an empty frame from image buffer";
            // 未读取到图片，后面所有流程都可以不用进行了
            std::this_thread::sleep_for(5ms);
            continue;
        }
        //图片为空
        if (raw_img.img.empty()) continue;
        //接收信号为空
        if (!_recv_buffer.Get(recv, raw_img.time_stamp))
        {
            //若在个人电脑上进行测试，可以使用默认数据进行初始化
            recv.init();
            //若没有收到电控数据，就无法进行预测，直接跳过此次循环，取下一个数据
            // continue;
        }

        //防止弹速为0时出现NaN计算错误
        if (static_cast<int>(recv.speed) <= 0) {
            recv.speed = BulletSpeed::infantry30;
        }

        // 在检测任务里，0代表蓝色， 1代表红色 ,2代表无
        // 在串口中, 接收到0代表无，1代表红色， 2代表蓝色
        int _enemy_color;
        if (recv.self_color == SelfColor::red)
            _enemy_color = 0;
        else if (recv.self_color == SelfColor::blue)
            _enemy_color = 1;
        else
            _enemy_color = 2;

        int _self_color;
#ifdef DEBUG_PARAMETERS
//        动态调参
        cv::imshow("parameters center", cv::Mat::zeros(1, 1000, CV_8UC3));
        cv::imshow("parameters center2", cv::Mat::zeros(1, 1000, CV_8UC3));
        _enemy_color = cv::getTrackbarPos("_enemy_color", "parameters center");
        _detector->min_lightness = cv::getTrackbarPos("min_lightness", "parameters center");
        _detector->l.min_ratio=float (cv::getTrackbarPos("light_min_ratio_x100", "parameters center"))/100;
        _detector->l.max_ratio=float (cv::getTrackbarPos("light_max_ratio_x100", "parameters center"))/100;
        _detector->l.max_angle=cv::getTrackbarPos("light_max_angle", "parameters center");
        _detector->a.min_light_ratio=float (cv::getTrackbarPos("armor_min_light_ratio_x10", "parameters center"))/10;
        _detector->a.min_small_center_distance=float (cv::getTrackbarPos("armor_min_small_center_distance_x10", "parameters center"))/10;
        _detector->a.max_small_center_distance=float (cv::getTrackbarPos("armor_max_small_center_distance_x10", "parameters center"))/10;
        _detector->a.min_large_center_distance=float (cv::getTrackbarPos("armor_min_large_center_distance_x10", "parameters center"))/10;
        _detector->a.max_large_center_distance=float (cv::getTrackbarPos("armor_max_large_center_distance_x10", "parameters center"))/10;
        _detector->a.max_angle=cv::getTrackbarPos("armor_max_angle", "parameters center");
        _compensator->channel_delay_ = float (cv::getTrackbarPos("channel_delay_x100", "parameters center2"))/100;
#endif

        //这里直接把弹速和颜色写死
        recv.speed = BulletSpeed::infantry30;
        _self_color = 0;

        //进行检测 vector<Armor>
        Energybuff result = Energybuff();
        // result.flabellums.resize(5);
        result = _detector->detect_buff(raw_img.img, _self_color);

        //画布
#ifdef DEBUG_SHOW
        cv::Mat canvas = raw_img.img.clone();
        _detector->drawResults(canvas);
        cv::namedWindow("detected", cv::WINDOW_AUTOSIZE);
        cv::imshow("detected", canvas);
#endif

        /*!
         *
         * @param armors_msg  std::vector<Armor>
         * @param recv_data   VisionRecvData
         * @param target_msg  TargetInfo
         */
        
        if (result.trackstate != Energybuff::TrackState::LOST && _detector->energybuff_.targetarmor.points2d.size() != 0)
        {
            _processor->ProcessBuff(_detector->energybuff_, recv, tmp_target);
            float yaw_armor = _compensator->CalcFinalAngle(tmp_target, recv, send, _processor->tracker->tracked_armors_num);

            if (abs(send.yaw - recv.yaw) < 1 && abs(send.pitch - recv.pitch) < 0.5 )
            {
                send.state = TargetState::fire;
            } else
            {
                send.state = TargetState::converging;
            }
            
            //增加偏置环节，消除物理误差
            float bias_pitch = 0, bias_yaw = 0;
#ifdef DEBUG_PARAMETERS
            bias_pitch += float (cv::getTrackbarPos("positive_bias_pitch_x100", "parameters center2"))/100;
            bias_yaw += float (cv::getTrackbarPos("positive_bias_yaw_x100", "parameters center2"))/100;
            bias_pitch -= float (cv::getTrackbarPos("negative_bias_pitch_x100", "parameters center2"))/100;
            bias_yaw -= float (cv::getTrackbarPos("negative_bias_yaw_x100", "parameters center2"))/100;
#endif
            send.yaw +=bias_yaw;
            send.pitch+=bias_pitch;
            _send_buffer.Update(send);
        } else {
            send.state = TargetState::lost_target;
            send = VisionSendData();
            _send_buffer.Update(send);
        }
#ifdef DEBUG_VOFA
        std::stringstream ss;
        ss << "debug:"
           << angles::to_degrees(tmp_target.yaw) << ","
           << tmp_target.radius_1 << "," << tmp_target.radius_2 << "," << tmp_target.w_yaw << ",";

        if (!result.empty())
        {
            ss << _processor->tracker->tracked_armor.position.x() << "," << _processor->tracker->tracked_armor.position.y() << ","
               << "0," << angles::to_degrees(_processor->tracker->orientationToYaw(_processor->tracker->tracked_armor.rotation));
        } else
        {
            ss << "0,0,0,0";
        }
        ss << "," << (int) _processor->tracker->tracker_state;
        ss << "," << send.yaw << "," << send.pitch;
        _tcp_logger->update(ss.str());
        _tcp_logger->log();
#endif

#ifdef DEBUG_SHOW
        cv::waitKey(1);
#endif
    }
}


[[noreturn]] void hnurm::ThreadManager::SerialRecvThread()
{
    while (true)
    {
#ifdef DEBUG_SHOW
        cv::namedWindow("RECV", cv::WINDOW_AUTOSIZE);
        cv::Mat RECV = cv::Mat::zeros(200, 300, CV_8UC3);
#endif

        VisionRecvData tmp_recv;
        if (_serial_codec->try_get_recv_data_for(tmp_recv))
        {
            auto get_time = std::chrono::steady_clock::now();
            _recv_buffer.Update(tmp_recv, get_time);
        } else {
            CLOG(DEBUG, "receive") << "read an empty recv info";
            std::this_thread::sleep_for(5ms);
        }
#ifdef DEBUG_SHOW
        cv::putText(RECV, "current pitch:" + std::to_string(tmp_recv.pitch), cv::Point(10, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
        cv::putText(RECV, "current yaw:" + std::to_string(tmp_recv.yaw), cv::Point(10, 40), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
        cv::putText(RECV, "current roll:" + std::to_string(static_cast<int>(tmp_recv.roll)), cv::Point(10, 60), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
        cv::putText(RECV, "current self_color:" + std::to_string(static_cast<int>(tmp_recv.self_color)), cv::Point(10, 80), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
        cv::putText(RECV, "current mode:" + std::to_string(static_cast<int>(tmp_recv.mode)), cv::Point(10, 100), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
        cv::putText(RECV, "current speed:" + std::to_string(static_cast<int>(tmp_recv.speed)), cv::Point(10, 120), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
        cv::imshow("RECV", RECV);
        cv::waitKey(1);
#endif
    }
}

[[noreturn]] void hnurm::ThreadManager::SerialSendThread()
{
    while (true)
    {
        VisionSendData data2send;

#ifdef DEBUG_SHOW
        cv::namedWindow("SEND", cv::WINDOW_AUTOSIZE);
        cv::Mat SEND = cv::Mat::zeros(200, 300, CV_8UC3);
#endif
        if (_send_buffer.Get(data2send))
        {
            _serial_codec->send_data(data2send);
        } else
        {
            CLOG(DEBUG, "send") << "fail to send information";
            std::this_thread::sleep_for(5ms);
        }
#ifdef DEBUG_SHOW
        cv::putText(SEND, "target pitch:" + std::to_string(data2send.pitch), cv::Point(10, 20), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
        cv::putText(SEND, "target yaw:" + std::to_string(data2send.yaw), cv::Point(10, 40), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
        cv::putText(SEND, "target status:" + std::to_string(static_cast<int>(data2send.state)), cv::Point(10, 60), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
        cv::putText(SEND, "target type:" + std::to_string(static_cast<int>(data2send.type)), cv::Point(10, 80), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1, 8, false);
        cv::imshow("SEND", SEND);
        cv::waitKey(1);
#endif
    }
}

void hnurm::ThreadManager::Exit()
{
    _camera->CloseCam();
}
